﻿#include "stackshap.h"

#include "stacksetting.h"
#include "../Algorithm/algorithm_log.h"

StackShap::StackShap()
{

}

StackShap::~StackShap()
{

}
using namespace stack;
/****************************************************************************
 * 功能: 垛形初始化
 * 传参: info:垛形配置信息
 * 返回值: 初始化是否成功
 ****************************************************************************/
bool StackShap::Init(const stack::StackCfg& info)
{
	m_id = info.id;
	m_name = info.name;
	m_max = info.maxNum;
	m_gap = info.gap;
	m_evenType = (EvenType)info.evenType;

	if (info.shap <= 0) {	// 固定尺寸垛形
		m_fixed = true;
		m_skuSize = info.sku;
		m_ceng = info.pos.size();
		m_skuPos = info.pos;
	} else {
		m_fixed = false;
		auto shapcfg = StackSetting::GetInstance()->GetShapCfgById(info.shap);
		if (shapcfg.id() <= 0) {
			log_error("shap error, shap = {}", info.shap);
			return false;
		}
		m_shap.max = shapcfg.aspectMax();
		m_shap.min = shapcfg.aspectMin();
		m_shap.lx = shapcfg.lenX();
		m_shap.ly = shapcfg.lenY();
		m_shap.wx = shapcfg.widX();
		m_shap.wy = shapcfg.widY();
		m_ceng = shapcfg.num();

		QList<stack::SkuShapPos> shappos;
		for (auto& iter : shapcfg.sku()) {
			stack::SkuShapPos sku = {};
			sku.id = iter.id();
			sku.group = iter.id();
			sku.lx = iter.lenX();
			sku.ly = iter.lenY();
			sku.wx = iter.widX();
			sku.wy = iter.widY();
			sku.typex = iter.typex();
			sku.typey = iter.typey();
			sku.za = iter.za();
			shappos.append(sku);
		}
		QList<SkuFixPos> fixpos;
		for (auto& iter : info.pos) {
			fixpos.append(iter.fix);
		}

		std::sort(fixpos.begin(), fixpos.end());
		std::sort(shappos.begin(), shappos.end());
		// 使用垛形配置中的组号以及旋转角度
		for (int i = 0; i < fixpos.size() && i < shappos.size(); i++) {
			shappos[i].group = fixpos[i].group;
			shappos[i].za = fixpos[i].za;
			stack::SkuPos pos;
			pos.shap = shappos[i];
			m_skuPos.append(pos);
		}
		m_ceng = m_skuPos.size();
	}
	log_trace("stack {} {} init success, ceng num = {}", m_id, m_name, m_ceng);
	return true;
}

/****************************************************************************
 * 功能: 校验物料尺寸和托盘是否适配垛形
 * 传参: sku: 物料尺寸信息 plate:托盘尺寸信息
 * 返回值: 适配成功返回最大码垛数量，不匹配返回-1；
 ****************************************************************************/
int StackShap::CheckSkuMatchStack(const ::Size& sku, const ::Size& plate)
{
	if (sku.height <= 0 || sku.length <= 0 || sku.width <= 0) {
		log_error("sku size error");
		return -1;
	}

	if (plate.height <= 0 || plate.length <= 0 || plate.width <= 0) {
		log_error("plate size error");
		return -1;
	}
	if (m_fixed) {
		if (std::abs(m_skuSize.length - sku.length) <= m_gap && std::abs(m_skuSize.width - sku.width) <= m_gap) {
			return MathMaxStackNum(plate.height, sku.height);
		} else {
			return -2;
		}
	} else {
		Size skugap = {};
		skugap.width = sku.width + m_gap;
		skugap.length = sku.length + m_gap;
		skugap.height = sku.height + m_gap;
		if (CheckSkuSizeForShap(sku, plate)) {
			return MathMaxStackNum(plate.height, sku.height);
		} else {
			return -2;
		}
	}
}

/****************************************************************************
 * 功能: 获取物料坐标
 * 传参: index: 抓取序号(1-n)，sku:物料尺寸 pos:输出坐标(z值无效) isOdd: 是否为奇数层
 * 返回值: 获取是否成功
 ****************************************************************************/
bool StackShap::GetStackPosForSku(int index, const Size& sku, stack::SkuPosInfo& pos, bool isOdd)
{
	if (index > m_ceng) {
		log_error("index > ceng");
		return false;
	}
	if (m_fixed) {
		if (std::abs(m_skuSize.length - sku.length) <= m_gap && std::abs(m_skuSize.width - sku.width) <= m_gap) {
			auto& temp = m_skuPos.at(index - 1).fix;
			pos.id = temp.id;
			pos.group = temp.group;
			pos.x = temp.x;
			pos.y = temp.y;
			pos.z = 1;
			pos.za = temp.za;
		} else {
			log_error("sku size not equal to stack sku size");
			return false;
		}
	} else {
		bool ret = GetShapStackPos(index, sku, pos, isOdd);
		if (!ret) {
			return false;
		}
	}
	if (isOdd) {		// 奇数层坐标直接返回
		return true;
	}

	switch (m_evenType) {
	case stack::EvenType::even_x_symmetry:	// x轴对称：y值变为相反数，y轴朝向的za旋转180
		pos.y = -pos.y;
		if (pos.za % 180 != 0) {
			pos.za += 180;
		}
		break;
	case stack::EvenType::even_y_symmetry: // y轴对称：x值变为相反数，x轴朝向的za旋转180；
		pos.x = -pos.x;
		if (pos.za % 180 == 0) {
			pos.za += 180;
		}
		break;
	case stack::EvenType::even_rotate_180:	// 绕垛形中心点旋转180
		pos.x = -pos.x;
		pos.y = -pos.y;
		pos.za += 180;
		break;
	case stack::EvenType::even_x_pos_symmetry:	// x轴位置对称，但旋转角度不变
		pos.y = -pos.y;
		break;
	case stack::EvenType::even_y_pos_symmetry:	// y轴位置对称，但旋转角度不变
		pos.x = -pos.x;
		break;

	default:
		break;
	}
	// za旋转设置在360范围内
	if (pos.za >= 360) {
		pos.za -= 360;
	}
	return true;
}


bool StackShap::CheckSkuSizeForShap(const Size& sku, const Size& plate)
{
	double aspect = (double)sku.length / sku.width;	// 计算长宽比
	if (aspect <= m_shap.min || aspect > m_shap.max) {	// 物料长宽不符合比例
		log_error("aspect error for {}", m_id);
		return false;
	}

	// 物料垛形X方向大于最大长度
	if (m_shap.lx * sku.length + m_shap.wx * sku.width > plate.length) {
		log_error("stack length over plate for {}", m_id);
		return false;
	}


	// 物料垛形y方向大于最大宽
	if (m_shap.ly * sku.length + m_shap.wy * sku.width > plate.width) {
		log_error("stack width over plate for {}", m_id);
		return false;
	}

	return true;
}
int StackShap::MathMaxStackNum(int maxH, int h)
{
	int math = maxH / h * m_ceng;
	if (m_max > 0) {
		return std::min(math, m_max);
	} else {
		return math;
	}
}
bool StackShap::GetShapStackPos(int index, const Size& skuin, stack::SkuPosInfo& pos, bool isOdd)
{
	auto& temp = m_skuPos.at(index - 1).shap;
	Size sku = {};
	sku.height = skuin.height;
	sku.width = skuin.width + m_gap;
	sku.length = skuin.length + m_gap;
	int w = sku.width / 2;
	int l = sku.length / 2;	// 物料半宽和半长

	int X = (m_shap.lx * sku.length + m_shap.wx * sku.width) / 2; //	垛形X长度的一半
	int Y = (m_shap.ly * sku.length + m_shap.wy * sku.width) / 2;	// 垛形Y长度的一半

	pos.group = temp.group;
	pos.id = temp.id;
	pos.za = temp.za;

	switch (temp.typex) {
	case 0:		// 左侧
		pos.x = (temp.wx * w + temp.lx * l) - X;
		break;
	case 1:		// 右侧
		pos.x = X - (temp.wx * w + temp.lx * l);
		break;
	case 2:		// 中左
		pos.x = -(temp.wx * w + temp.lx * l);
		break;
	case 3:		// 中右
		pos.x = (temp.wx * w + temp.lx * l);
		break;
	default:
		log_error("stack typex error");
		return false;
	}

	switch (temp.typey) {
	case 0:
		pos.y = (temp.wy * w + temp.ly * l) - Y;
		break;
	case 1:
		pos.y = Y - (temp.wy * w + temp.ly * l);
		break;
	case 2:
		pos.y = -(temp.wy * w + temp.ly * l);
		break;
	case 3:
		pos.y = (temp.wy * w + temp.ly * l);
		break;
	default:
		log_error("stack typey error");
		return false;
	}
	return true;
}
